/// 相机内参计算，直接使用opencv自己带的
/// f u0, v0 计算
/// 需要两个平面才能计算出来
///
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include <memath/meTransform.h>
#include <memath/MeEigenMath.h>

using namespace std;

// 数据来自于一影采集的2D图像，图像大小为2064*2064
// 二维标尺有上下两个面，采集两次得到4个面的数据
const static double s_objPoints[] = {
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane2
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
    //55.06, 5.75, 2,
    // plane3
    33.50, 73.77, 78,
    -3.27, 37, 78,
    33.50, 0.23, 78,
    70.27, 37, 78,
    33.50, 63.77, 78,
    // plane4
    67, 7, 2,
    0, 0, 2,
    0, 74, 2,
    67, 67, 2,
    71.95, 2.05, 2,
};

const static double s_imagePoints[] = {
    558.791 ,1481.58,
    175.822 ,901.062,
    754.578 ,534.524,
    1130.44 ,1109.1,
    585.582 ,1352.91,
    // plane2
    1317.02 ,573.621,
    386.046 ,264.715,
    166.893 ,1322.2,
    1147.49 ,1423.43,
    1401.28 ,520.14,
    //1152.33 ,519.293,

    // plane3
    1290.68 ,1513.81,
    783.605 ,1045.01,
    1254.05 ,542.663,
    1757.41 ,1011.03,
    1286.35 ,1381.44,
    // plane4
    1782.11 ,544.629,
    801.412 ,479.553,
    845.876 ,1559.89,
    1818.47 ,1418.17,
    1851.84 ,470.476,
    //1607.66 ,532.807,
};


void createRuler2DRealData(vector<cv::Point2f> &objPts, vector<cv::Point2f> &imagePts, const int nPlane = 0)
{
    int sIndex = 5*nPlane;
    int nPts = 5;

    const double *p1 = s_objPoints+3*sIndex;
    const double *p2 = s_imagePoints+2*sIndex;
    objPts.clear();
    imagePts.clear();
    for (int i=0; i < nPts; i++) {
        objPts.push_back(cv::Point2f(p1[i*3+0], p1[i*3+1]));
        imagePts.push_back(cv::Point2f(p2[i*2+0], p2[i*2+1]));
    }
}

void createRuler2DRealData2(vector<cv::Point3f> &objPts, vector<cv::Point2f> &imagePts, const int nPlane = 0)
{
    int sIndex = 5*nPlane;
    int nPts = 5;

    const double *p1 = s_objPoints+3*sIndex;
    const double *p2 = s_imagePoints+2*sIndex;
    objPts.clear();
    imagePts.clear();
    for (int i=0; i < nPts; i++) {
        objPts.push_back(cv::Point3f(p1[i*3+0], p1[i*3+1], 0));
        imagePts.push_back(cv::Point2f(p2[i*2+0], p2[i*2+1]));
    }
}

/// 计算相机内参矩阵
cv::Mat computeCameraMatrix(const int nPlane)
{
    vector<vector<cv::Point2f> > imgPts(nPlane);
    vector<vector<cv::Point3f> > objPts(nPlane);
    for (uint i=0; i<nPlane; i++) {
        createRuler2DRealData2(objPts[i], imgPts[i], i);
        //cout << "objPts: " << objPts[i] << endl;
        //cout << "imgPts: " << imgPts[i] << endl;
    }
    cv::Size imageSize(2064, 2064);
    cv::Mat cM = cv::initCameraMatrix2D(objPts, imgPts, imageSize, 0);
    cout << "camera Matrix:\n" << cM << endl;
    return cM;
}

/// 计算相机内外参数
void computeCameraInExMatrix()
{
    const int nPlane = 4;
    // 用标尺的两个面计算内参
    cv::Mat A = computeCameraMatrix(4);

    cv::Mat invA = A.inv();

    // 根据给定数据计算单应矩阵H
    vector<vector<cv::Point2f> > imgPts(nPlane);
    vector<vector<cv::Point2f> > objPts(nPlane); // 为三维坐标z=0的值
    cv::Mat H[nPlane];
    for (uint i=0; i<nPlane; i++) {
        cout << "plane " << i << endl;
        createRuler2DRealData(objPts[i], imgPts[i], i);
        // cout << "objPts: " << objPts[i] << endl;
        // cout << "imgPts: " << imgPts[i] << endl;
        H[i] = cv::findHomography(objPts[i], imgPts[i]);
        cout << "H:\n" << H[i] << endl;

        // 计算外参
        cv::Mat R1 = invA * H[i];
        // cout << "R1:\n" << R1 << endl;
        double *pR1 = R1.ptr<double>();


        double k1 = sqrt(pR1[0]*pR1[0] + pR1[3+0]*pR1[3+0] + pR1[6+0]*pR1[6+0]);
        double k2 = sqrt(pR1[1]*pR1[1] + pR1[3+1]*pR1[3+1] + pR1[6+1]*pR1[6+1]);

        double r1[] = {pR1[0]/k1, pR1[3]/k1, pR1[6]/k1};
        double r2[] = {pR1[1]/k2, pR1[4]/k2, pR1[7]/k2};
        double r3[3];
        MeMath::cross(r1, r2, r3);
        //cout << "k:" << k1 << ", " << k2 << endl;

        // 计算相机和世界坐标系的转化矩阵
        cv::Mat R = cv::Mat(4, 4, CV_64F, 1);
        R.at<double>(0, 0) = R1.at<double>(0, 0)/k1;
        R.at<double>(1, 0) = R1.at<double>(1, 0)/k1;
        R.at<double>(2, 0) = R1.at<double>(2, 0)/k1;

        R.at<double>(0, 1) = R1.at<double>(0, 1)/k2;
        R.at<double>(1, 1) = R1.at<double>(1, 1)/k2;
        R.at<double>(2, 1) = R1.at<double>(2, 1)/k2;

        R.at<double>(0, 2) = r3[0];
        R.at<double>(1, 2) = r3[1];
        R.at<double>(2, 2) = r3[2];

        R.at<double>(0, 3) = R1.at<double>(0, 2)/k2;
        R.at<double>(1, 3) = R1.at<double>(1, 2)/k2;
        R.at<double>(2, 3) = R1.at<double>(2, 2)/k2;

        cout << "R:\n" << R << endl;
    }
}

int main(int argc, char *argv[])
{
    if (argc < 2) {
        std::cerr << "Usage: " << std::endl;
        std::cerr << argv[0] << "option\n";
        std::cerr << "option:\n"
                     "   0 - only verify intrinsic matric\n"
                     "   1 - compute A, [R,t]\n";
        return EXIT_FAILURE;
    }
    int option = atoi(argv[1]);
    if (option == 0) {
        cout << "===使用 initCameraMatrix2D 计算相机fx和fy.===" << endl;

        cout << "只有一个面，可计算fx，fy: " << endl;
        computeCameraMatrix(1);
        cout << "--- " << endl;

        cout << "2个面，计算fx，fy: " << endl;
        computeCameraMatrix(2);
        cout << "--- " << endl;

        cout << "4个面，计算fx，fy: " << endl;
        computeCameraMatrix(4);
        cout << "--- " << endl;
    } else if (option == 1) {

        computeCameraInExMatrix();
    }


    return 0;
}



